package myVecmathImp;

import java.nio.FloatBuffer;

import org.lwjgl.BufferUtils;

import ogl.vecmath.Matrix;
import ogl.vecmath.Vector;
import ogl.vecmathimp.MatrixImp;
import static ogl.vecmathimp.FactoryDefault.vecmath;

public class MyMatrix implements Matrix {

	private float[][] values;

	public MyMatrix() {
		values = new float[][] { { 1, 0, 0, 0 }, { 0, 1, 0, 0 },
				{ 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
	}

	public MyMatrix(float[][] vals) {
		values = new float[4][4];
		setValues2(vals);
	}

	public MyMatrix(float[] vals) {
		values = new float[4][4];
		setValues(vals);
	}

	public static String toString1(float[] val) {
		String result = "";

		for (int i = 0; i < 16; i++) {
			float value = val[i];

			// depending on the number of numbers of the current value,
			// make the gap fit accordingly to ensure that the output looks
			// well formatted
			String gap = "";
			if (value < 10 && value > -10) {
				gap = "    ";
			} else if (value > 999 || value < -999) {
				gap = " ";
			} else if (value > 99 || value < -99) {
				gap = "  ";
			} else
				gap = "   ";

			if ((i + 1) % 4 == 0) {
				result += value + gap + "\n";
			} else {
				result += value + gap;
			}
		}

		return result;
	}

	public String toString() {
		String result = "";

		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				float value = values[i][j];

				// depending on the number of numbers of the current value,
				// make the gap fit accordingly to ensure that the output looks
				// well formatted
				String gap = "";
				if (value < 10 && value > -10) {
					gap = "    ";
				} else if (value > 999 || value < -999) {
					gap = " ";
				} else if (value > 99 || value < -99) {
					gap = "  ";
				} else
					gap = "   ";

				result += value + gap;
			}

			result += "\n";
		}

		return result;
	}

	private void set(int c, int r, float val) {
		this.values[r][c] = val;
	}

	@Override
	public float[] getValues() {
		float[] val = new float[16];

		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				val[i * 4 + j] = values[i][j];
				System.out.println("i = " + i + "  j = " + j);
			}
		}
		return val;
	}

	public float[][] getValues2() {
		return values;
	}

	@Override
	public void setValues(float[] vals) {
		if (vals.length == 16) {
			for (int i = 0; i < 16; i++) {
				values[i / 4][i % 4] = vals[i];
			}
		} else
			System.err
					.println("Please pass an array with 16 values! You passed "
							+ vals.length + " values.");
	}

	private void setValues2(float[][] vals) {
		if (vals.length == 4 && vals[0].length == 4 && vals[1].length == 4
				&& vals[2].length == 4 && vals[3].length == 4) {
			for (int i = 0; i < 4; i++) {
				for (int j = 0; j < 4; j++) {
					values[i][j] = vals[i][j];
				}
			}
		} else
			System.err.println("Please pass an array with 4x4 values!");

	}

	@Override
	public float get(int c, int r) {
		if (c > 3 || c < 0) {
			System.err.println("Please enter a valid column value!");
			return -100000;
		}
		if (r > 3 || r < 0) {
			System.err.println("Please enter a valid row value!");
			return -100000;
		}
		return values[r][c];
	}

	@Override
	public Matrix mult(Matrix m) {
		float[][] a = values;
		float[][] b = m.getValues2();

		float[][] result = new float[4][4];

		// calculate first row
		result[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2]
				* b[2][0] + a[0][3] * b[3][0];
		result[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2]
				* b[2][1] + a[0][3] * b[3][1];
		result[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2]
				* b[2][2] + a[0][3] * b[3][2];
		result[0][3] = a[0][0] * b[0][3] + a[0][1] * b[1][3] + a[0][2]
				* b[2][3] + a[0][3] * b[3][3];

		// calculate second row
		result[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2]
				* b[2][0] + a[1][3] * b[3][0];
		result[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2]
				* b[2][1] + a[1][3] * b[3][1];
		result[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2]
				* b[2][2] + a[1][3] * b[3][2];
		result[1][3] = a[1][0] * b[0][3] + a[1][1] * b[1][3] + a[1][2]
				* b[2][3] + a[1][3] * b[3][3];

		// calculate third row
		result[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2]
				* b[2][0] + a[2][3] * b[3][0];
		result[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2]
				* b[2][1] + a[2][3] * b[3][1];
		result[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2]
				* b[2][2] + a[2][3] * b[3][2];
		result[2][3] = a[2][0] * b[0][3] + a[2][1] * b[1][3] + a[2][2]
				* b[2][3] + a[2][3] * b[3][3];

		// calculate fourth row
		result[3][0] = a[3][0] * b[0][0] + a[3][1] * b[1][0] + a[3][2]
				* b[2][0] + a[3][3] * b[3][0];
		result[3][1] = a[3][0] * b[0][1] + a[3][1] * b[1][1] + a[3][2]
				* b[2][1] + a[3][3] * b[3][1];
		result[3][2] = a[3][0] * b[0][2] + a[3][1] * b[1][2] + a[3][2]
				* b[2][2] + a[3][3] * b[3][2];
		result[3][3] = a[3][0] * b[0][3] + a[3][1] * b[1][3] + a[3][2]
				* b[2][3] + a[3][3] * b[3][3];

		return new MyMatrix(result);
	}

	@Override
	public Matrix multSlow(Matrix m) {
		float[][] a = values;
		float[][] b = m.getValues2();
		float[][] val = new float[4][4];

		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				for (int z = 0; z < 4; z++) {
					val[i][j] += a[i][z] * b[z][j];
				}
			}
		}
		return new MyMatrix(val);
	}

	@Override
	public Vector transformPoint(Vector v) {
		float x = v.x() * values[0][0] + v.y() * values[0][1] + v.z()
				* values[0][2] + values[0][3];
		float y = v.x() * values[1][0] + v.y() * values[1][1] + v.z()
				* values[1][2] + values[1][3];
		float z = v.x() * values[2][0] + v.y() * values[2][1] + v.z()
				* values[2][2] + values[2][3];
		return vecmath.vector(x, y, z);
	}

	@Override
	public Vector transformDirection(Vector v) {
		float x = v.x() * values[0][0] + v.y() * values[0][1] + v.z()
				* values[0][2];
		float y = v.x() * values[1][0] + v.y() * values[1][1] + v.z()
				* values[1][2];
		float z = v.x() * values[2][0] + v.y() * values[2][1] + v.z()
				* values[2][2];
		return vecmath.vector(x, y, z);
	}

	@Override
	public Vector transformNormal(Vector v) {
		return transformDirection(v);
	}

	@Override
	public Matrix transpose() {
		MyMatrix m = new MyMatrix();
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				m.values[i][j] = get(i, j);
			}
		}
		return m;
	}

	@Override
	public Matrix invertRigid() {
		MyMatrix ri = new MyMatrix();
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j != 3; j++) {
				ri.set(i, j, get(j, i));
			}
		}
		MyMatrix ti = new MyMatrix();
		ti.values[0][3] = -get(3, 0);
		ti.values[1][3] = -get(3, 1);
		ti.values[2][3] = -get(3, 2);
		return ri.mult(ti);
	}

	@Override
	public Matrix invertFull() {
		Matrix ret = new MyMatrix();
		float[] mat = getValues();
		float[] dst = ret.getValues();
		float[] tmp = new float[12];

		/* temparray for pairs */
		float src[] = new float[16];

		/* array of transpose source matrix */
		float det;

		/* determinant */
		/*
		 * transpose matrix
		 */
		for (int i = 0; i < 4; i++) {
			src[i] = mat[i * 4];
			src[i + 4] = mat[i * 4 + 1];
			src[i + 8] = mat[i * 4 + 2];
			src[i + 12] = mat[i * 4 + 3];
		}

		/* calculate pairs for first 8 elements (cofactors) */
		tmp[0] = src[10] * src[15];
		tmp[1] = src[11] * src[14];
		tmp[2] = src[9] * src[15];
		tmp[3] = src[11] * src[13];
		tmp[4] = src[9] * src[14];
		tmp[5] = src[10] * src[13];
		tmp[6] = src[8] * src[15];
		tmp[7] = src[11] * src[12];
		tmp[8] = src[8] * src[14];
		tmp[9] = src[10] * src[12];
		tmp[10] = src[8] * src[13];
		tmp[11] = src[9] * src[12];

		/* calculate first 8 elements (cofactors) */
		dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
		dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
		dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
		dst[1] -= tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7];
		dst[2] = tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7];
		dst[2] -= tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7];
		dst[3] = tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6];
		dst[3] -= tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6];
		dst[4] = tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3];
		dst[4] -= tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3];
		dst[5] = tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3];
		dst[5] -= tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3];
		dst[6] = tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3];
		dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
		dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
		dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];

		/* calculate pairs for second 8 elements (cofactors) */
		tmp[0] = src[2] * src[7];
		tmp[1] = src[3] * src[6];
		tmp[2] = src[1] * src[7];
		tmp[3] = src[3] * src[5];
		tmp[4] = src[1] * src[6];
		tmp[5] = src[2] * src[5];
		tmp[6] = src[0] * src[7];
		tmp[7] = src[3] * src[4];
		tmp[8] = src[0] * src[6];
		tmp[9] = src[2] * src[4];
		tmp[10] = src[0] * src[5];
		tmp[11] = src[1] * src[4];

		/* calculate second 8 elements (cofactors) */
		dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
		dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
		dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
		dst[9] -= tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15];
		dst[10] = tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15];
		dst[10] -= tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15];
		dst[11] = tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14];
		dst[11] -= tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14];
		dst[12] = tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9];
		dst[12] -= tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10];
		dst[13] = tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10];
		dst[13] -= tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8];
		dst[14] = tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8];
		dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
		dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
		dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];

		/* calculate determinant */
		det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3]
				* dst[3];

		if (det == 0.0f) {
			throw new RuntimeException("singular matrix is not invertible");
		}

		/* calculate matrix inverse */
		det = 1 / det;

		for (int j = 0; j < 16; j++) {
			dst[j] *= det;
		}

		return ret;

	}

	@Override
	public float[] asArray() {
		return getValues().clone();
	}

	@Override
	public FloatBuffer asBuffer() {
		FloatBuffer b = BufferUtils.createFloatBuffer(getValues().length);
		b.put(getValues());
		b.rewind();
		return b;
	}

	@Override
	public void fillBuffer(FloatBuffer buf) {
		buf.rewind();
		buf.put(getValues()).rewind();
	}

	@Override
	public Matrix getRotation() {
		MyMatrix r = new MyMatrix();

		r.values[0][0] = get(0, 0);
		r.values[0][1] = get(1, 0);
		r.values[0][2] = get(2, 0);
		r.values[1][0] = get(0, 1);
		r.values[1][1] = get(1, 1);
		r.values[1][2] = get(2, 1);
		r.values[2][0] = get(0, 2);
		r.values[2][1] = get(1, 2);
		r.values[2][2] = get(2, 2);

		return r;
	}

	@Override
	public Matrix getTranslation() {
		MyMatrix t = new MyMatrix();
		t.values[0][3] = get(3, 0);
		t.values[1][3] = get(3, 1);
		t.values[2][3] = get(3, 2);
		return t;
	}

	@Override
	public Vector getPosition() {
		return vecmath.vector(get(3, 0), get(3, 1), get(3, 2));
	}

	@Override
	public boolean equals(Matrix m, float epsilon) {
		for (int i = 0; i != 16; i++)
			if (Math.abs(getValues()[i] - m.getValues()[i]) > epsilon)
				return false;
		return true;
	}

	/**
	 * @param args
	 */
	public static void main(String[] args) {
		MyMatrix m = new MyMatrix(new float[][] { { 9, 0, 0, 0 },
				{ 0, 7, 5, 0 }, { 0, 0, 1, 18 }, { 4, 0, 0, 1 } });
		MyMatrix m2 = new MyMatrix(new float[] { 0, 0, 12, 0, 0, 1, 0, 0, 0, 7,
				0, 8, 4, 0, 0, 0 });
		System.out.println(m.toString());
		System.out.println(toString1(m.getValues()));

		System.out.println(m2.toString());
		MyMatrix m3 = (MyMatrix) m.multSlow(m2);
		MyMatrix m4 = (MyMatrix) m.mult(m2);
		System.out.println(m3.toString());
		System.out.println(m4.toString());

	}

}
